#!/usr/bin/python
# -*- coding: UTF-8 -*-

import time

class BwRobotLib:
    def __init__(self, control_lib):
        self.mclib = control_lib
        # 当前连接的机器人IP与端口Port
        # 串口位置ID与串口名称映射
        self.LOCATION_NAME_2NUM_MAP = {'F_DOWN': 1, 'F_UP': 2, \
                                       'P_L1': 8, 'P_L2': 4, 'P_L3': 10, 'P_R1': 7, 'P_R2': 11, 'P_R3': 6, 'P_UP': 9, \
                                       'M_L1': 8, 'M_L2': 8, 'M_L3': 10, 'M_L4': 11, 'M_R1': 4, 'M_R2': 7, 'M_R3': 6, 'M_R4': 6, 'M_FORWARD': 4, 'M_BACK': 11, 'M_UP': 9}

    ######################################################################
    ################# 机器人构型ID配置 ##################################
    ######################################################################
    def loadConfig(self, configPath='config.txt'):
        temp = []
        module_info = []
        # 加载Config文件
        print("读取ID配置文件:"+configPath)
        f = open(configPath, 'r')
        lines = f.readlines()
        f.close()
        for row in lines:  
            row = row.replace('\n', '')  
            # print(row)    
            # 以 # 开头的行作为注释行
            if(row.startswith('#') is False):
                row = row.split(",")  # 去除换行符
                for i in row:
                    temp.append(int(i))
                module_info.append(temp)
            temp = []
        if len(module_info) == 0 :
            print("请输入机器人模块ID！")
            return 
        elif len(module_info) == 1 :
            module_ids = module_info[0] 
            module_ids_loc =[0]*len(module_ids)
            print("机器人模块信息导入正确！")
            return module_ids, module_ids_loc
        elif len(module_info) == 2 :
            module_ids = module_info[0] 
            module_ids_loc =module_info[1]
            if (len(module_ids) == len(module_ids_loc)):
                print("机器人模块信息导入正确！")
                return module_ids, module_ids_loc
            else:
                print("机器人模块ID和连接信息对应不正确！")
        else:
            print("输入数据格式错误！")
            return 

    

     ######################################################################
     ################# 根据位置检索ID列表 #################################
     ######################################################################
    def queryIdsByLoc(self, pos, ids, postions, default=[0]):
        targetIds = []
        size = len(postions)
        for i in range(size):
            if(postions[i] == pos):
                targetIds.append(ids[i])
            i = i + 1
        if(len(targetIds) == 0):
            targetIds = default
        return targetIds

     ######################################################################
     ######################## 扫描并配置XWheel机器人ID ####################
     ######################################################################

    def scanNConfigXWheelIds(self):
        idPs = self.mclib.ScanId()
        intIds = idPs[0]
        intPos = idPs[1]
        # L1 L3 R1 R3
        sortIds = []

        l1Ids = self.queryIdsByLoc(
            self.LOCATION_NAME_2NUM_MAP['P_L1'], idPs[0], idPs[1])
        l3Ids = self.queryIdsByLoc(
            self.LOCATION_NAME_2NUM_MAP['P_L3'], idPs[0], idPs[1])
        r1Ids = self.queryIdsByLoc(
            self.LOCATION_NAME_2NUM_MAP['P_R1'], idPs[0], idPs[1])
        r3Ids = self.queryIdsByLoc(
            self.LOCATION_NAME_2NUM_MAP['P_R3'], idPs[0], idPs[1])
        # if(len(l1Ids) != 1 or len(l3Ids) != 1 or len(r1Ids) != 1 or len(r3Ids) != 1):
        #    return sortIds

        sortIds = [l1Ids[0], l3Ids[0], r1Ids[0], r3Ids[0]]
        return sortIds

     ######################################################################
     ######################## 扫描并配置小车Car机器人ID ####################
     ######################################################################

    def scanNConfigCarIds(self,print_flag=False):
        idPs = self.mclib.ScanId()
        intIds = idPs[0]
        intPos = idPs[1]
        if print_flag == True:
            for i in range(len(intIds)):
                print("模块ID: "+str(intIds[i])+" 位置："+str(intPos[i]))

        fIds = self.queryIdsByLoc(4, idPs[0], idPs[1])
        bIds = self.queryIdsByLoc(11, idPs[0], idPs[1])
        l1Ids = self.queryIdsByLoc(8, idPs[0], idPs[1])
        l2Ids = self.queryIdsByLoc(8, idPs[0], idPs[1])
        l3Ids = self.queryIdsByLoc(10, idPs[0], idPs[1])
        l4Ids = self.queryIdsByLoc(11, idPs[0], idPs[1])

        r1Ids = self.queryIdsByLoc(4, idPs[0], idPs[1])
        r2Ids = self.queryIdsByLoc(7, idPs[0], idPs[1])
        r3Ids = self.queryIdsByLoc(6, idPs[0], idPs[1])
        r4Ids = self.queryIdsByLoc(6, idPs[0], idPs[1])
        sortIds = [fIds[0], bIds[0], l1Ids[0], l2Ids[0], l3Ids[0],
                   l4Ids[0], r1Ids[0], r2Ids[0], r3Ids[0], r4Ids[0]]
        return sortIds

